#! /usr/bin/python

__author__="unlotto"
__date__ ="$Feb 14, 2009 11:44:39 AM$"

import sys
import lpantilt
from opencv import highgui
from opencv import cvGetMat
from opencv.adaptors import Ipl2PIL

import pygame
from pygame.locals import *
from time import time

from multiprocessing import Process, Value

VERBOSE = False
FPS = 25
CAMFPS = 25

WIDTH=640
HEIGHT=480


#a few constant propterty ids for device properties
CV_CAP_PROP_FRAME_WIDTH = 3
CV_CAP_PROP_FRAME_HEIGHT = 4
CV_CAP_PROP_FPS = 6
CV_CAP_PROP_BRIGHTNESS = 8
CV_CAP_PROP_CONTRAST = 9
CV_CAP_PROP_SATURATION = 10
CV_CAP_PROP_HUE = 11

class Timer:
  """small class for runtime calcs"""
  def __init__(self):
    self.start()
  def start(self):
    self.startTime = round(time(),3)

  def stop(self,message):
    if VERBOSE:
      self.stopTime = round(time(),3)
      print message + "  "+str(round((self.stopTime-self.startTime),3))

  def fps(self,message):
    if VERBOSE:
      self.stopTime = round(time(),3)
      print message + "  "+str(round(1/round((self.stopTime-self.startTime),4),3))


class Webcam():
  """creates a small pygame window"""
  camera = None #camera device
  screen = None #screen

  def __init__(self):

    pygame.init()
    window = pygame.display.set_mode((WIDTH,HEIGHT))
    pygame.display.set_caption("Lpantilt example/test")
    self.screen = pygame.display.get_surface()
    
    self.STATE = Value('i', 0)
    self.general = CommandDispatcher(lpantilt.Lpantilt("/dev/video1"),self.STATE)
    
    self.general.start()
    
    self.camera = highgui.cvCreateCameraCapture(1)

    #Set camera settings
    
    #setting many options will softfail. No opencv support yet. Try anyway
    highgui.cvSetCaptureProperty( self.camera, CV_CAP_PROP_FRAME_WIDTH, WIDTH)
    highgui.cvSetCaptureProperty( self.camera, CV_CAP_PROP_FRAME_HEIGHT, HEIGHT)
    highgui.cvSetCaptureProperty( self.camera, CV_CAP_PROP_FPS, CAMFPS)

    #get width and height as reported by cam
    self.camwidth = int(highgui.cvGetCaptureProperty( self.camera, CV_CAP_PROP_FRAME_WIDTH))
    self.camheight = int(highgui.cvGetCaptureProperty( self.camera, CV_CAP_PROP_FRAME_HEIGHT))

    self.clock = pygame.time.Clock()
    self.enter_loop()

  def enter_loop(self):
    """Enter main pygame frames loop"""

    #local vars and functions for speed
    #this probably doesnt matter.
    camwidth = self.camwidth
    camheight = self.camheight
    
    localIpl2Pil = Ipl2PIL
    localCvGetMat = cvGetMat
    localQueryFrame = highgui.cvQueryFrame

    localImageFromBuffer = pygame.image.frombuffer
    localScale = pygame.transform.scale

    localFlip = pygame.display.flip

    needScale = False
    if WIDTH != camwidth:
      needScale = True

    tot = Timer()
    while True:
      tot.start()
      camshot = localIpl2Pil(localQueryFrame(self.camera))
      self.handleEvents()

      camshot = localImageFromBuffer(camshot.tostring(), (camwidth,camheight), "RGB")

      #only scale if needed
      if needScale:
        camshot = localScale(camshot, (WIDTH,HEIGHT))

      #pygame.surfarray.blit_array(screen, pygame.surfarray.array3d(camshot))
      self.screen.blit(camshot,(0,0))
      localFlip()
      self.clock.tick(FPS)
      if VERBOSE:
        print "Curent FPS = "+str(self.clock.get_fps())
      
      tot.stop("Total Runtime:")   


  def exit(self):
    self.STATE.value = -1
    try:
      self.lpt.closedev()
      highgui.cvReleaseCapture(self.camera) #I don't know if i'm supposed to do this????
    except AttributeError, e:
      pass
    sys.exit(0)

  def handleEvents(self):
    events = pygame.event.get()
    for event in events:

      if event.type == QUIT:
        self.exit()

      if event.type is KEYDOWN:        
        keyname = pygame.key.name(event.key)
        print keyname

        pygame.event.set_blocked(KEYDOWN)

        self.STATE.value = 0
        
        
        if keyname == "left":
          self.STATE.value = 1
        elif keyname == "right":        
          self.STATE.value = 2
        elif keyname == "up":          
          self.STATE.value = 3
        elif keyname == "down":
          self.STATE.value = 4
        elif keyname == "q":
          self.exit()

        pygame.event.set_allowed(KEYDOWN)
        
      elif event.type is KEYUP:
        keyname = pygame.key.name(event.key)
        print keyname
        
        self.STATE.value = 0

class CommandDispatcher(Process):
  """We share state using ctype Values"""
  CONSTANT_LEFT = 1
  CONSTANT_RIGHT = 2
  CONSTANT_UP = 3
  CONSTANT_DOWN = 4
  
  def __init__(self,lpt,STATE):
    self.STATE = STATE
    self.lpt = lpt
    self.lpt.reset()
    self.lpt.wait = False

    Process.__init__(self);

  def run(self):
    while True:
      if self.STATE.value == self.CONSTANT_LEFT:
        self.lpt.pan(2)
      elif self.STATE.value == self.CONSTANT_RIGHT:
        self.lpt.pan(-2)
      elif self.STATE.value == self.CONSTANT_UP:
        self.lpt.tilt(-2)
      elif self.STATE.value == self.CONSTANT_DOWN:
        self.lpt.tilt(2)
      elif self.STATE.value == -1: #allow for process to be terminated by parent
        break;
    

if __name__ == "__main__":
  webcam = Webcam()
